import matplotlib.pyplot as plt
import numpy as np
import math

class DataDisplayer:
  dataList = []

  @staticmethod
  def add(data):
    DataDisplayer.dataList.append(data)

  @staticmethod
  def display():
    all_data = DataDisplayer.dataList
    num = len(all_data)
    for i in range(num):
      the_data = all_data[i]
      plt.subplot(1, num, i+1)
      plt.ylabel(the_data.name)
      if isinstance(the_data, SensorData):
        plt.plot(the_data.T, the_data.X)
        plt.plot(the_data.T, the_data.Y)
        plt.plot(the_data.T, the_data.Z)
      elif isinstance(the_data, Data):
        for i in range(len(the_data.dims) - 1):
          plt.plot(the_data.dims[0], the_data.dims[i+1])
      else:
        print('error type')
    plt.show()

class SensorData:
  
  def __init__(self, name):
    self.T = np.array([])
    self.X = np.array([])
    self.Y = np.array([])
    self.Z = np.array([])

    self.name = name
  
  def append(self, t, x, y, z):
    self.T = np.append(self.T, t)
    self.X = np.append(self.X, x)
    self.Y = np.append(self.Y, y)
    self.Z = np.append(self.Z, z)

  def size(self):
    return len(self.T)

class Data:
  def __init__(self, name, dim):
    self.name = name
    self.dims = []
    for i in range(dim):
      self.dims.append(np.array([]))

class RawDataPaser:
  @staticmethod
  def parse(raw):

    accData = SensorData('acc')
    gravityData = SensorData('gravity')
    gyroData = SensorData('gyro')

    for line in raw.split('\n'):
      params = line.split(' ')
      if len(params) != 5:
        continue
      name = params[1]
      t = float(params[0])
      x = float(params[2])
      y = float(params[3])
      z = float(params[4])

      toAppend = None
      if name == 'acc':
        toAppend = accData
      elif name == 'gravity':
        toAppend = gravityData
      elif name == 'gyro':
        toAppend = gyroData

      toAppend.append(t, x, y, z)
      # print(f'{accData.size()} -----')
    return accData, gravityData, gyroData

class FileReader:
  @staticmethod
  def read(path):
    file = open(path)
    data_string = file.read()
    return data_string

if __name__ == '__main__':
  data_str = FileReader.read('data/20200402_11_30_30.txt')
  data = RawDataPaser.parse(data_str)

  # 线性加速度计
  # DataDisplayer.add(data[0])

  # 重力计
  # DataDisplayer.add(data[1])

  # 陀螺仪
  # DataDisplayer.add(data[2])

  # 线性加速度大小
  data_acc_sum = Data('acc_sum', 2)
  data_acc_sum.dims[0] = data[0].T
  sum_array = []
  for i in range(len(data_acc_sum.dims[0])):
    s = math.sqrt(data[0].X[i] ** 2 + data[0].Y[i] ** 2 + data[0].Z[i] ** 2)
    sum_array.append(s)
  data_acc_sum.dims[1] = np.array(sum_array)
  DataDisplayer.add(data_acc_sum)

  # 线性加速度大小 - 算术平均值滤波
  data_acc_filterd = Data('acc_filterd', 2)
  data_acc_filterd.dims[0] = data_acc_sum.dims[0][8:-8]
  filter_array = []

  def filter(arr, filter_len):
    '''len 是算术平均值取值的数量'''
    if filter_len % 2 != 1:
      raise Exception('滤波器长度为奇数')
  
    half_len = (filter_len-1) / 2

    filterd = []

    for i in range(len(arr) - filter_len + 1):
      sum = 0
      for j in range(filter_len):
        sum += arr[i+j]
      filterd.append(sum / filter_len)
    
    return filterd

  data_acc_filterd.dims[1] = np.array(filter(data_acc_sum.dims[1], 17))
  DataDisplayer.add(data_acc_filterd)

  DataDisplayer.display()

# # 初始化4维
# T = np.array([])
# X = np.array([])
# Y = np.array([])
# Z = np.array([])
# SUM = np.array([])

# # 读取数据
# file = open('data.log')
# data_string = file.read()
# for line in data_string.split('\n'):
#   params = line.split('#')
#   if len(params) != 5:
#     continue

#   t = float(params[4])

#   x = float(params[1])
#   y = float(params[2])
#   z = float(params[3])

#   sum = math.sqrt(x**2+y**2+z**2)

#   X = np.append(X, x)
#   Y = np.append(Y, y)
#   Z = np.append(Z, z)
#   T = np.append(T, t)
#   SUM = np.append(SUM, sum)


# # 时间处理
# t0 = int(T[0])
# for i in range(len(T)):
#   T[i] = (int(T[i]) - t0) / 1000000000
# # t1 = int(T[len(T)-1])
# # dt = t1 - t0
# # print(dt/1000000)

# # 数据初始化
# plt.plot(T,X, label='X')
# plt.plot(T,Y, label='Y')
# plt.plot(T,Z, label='Z')
# plt.plot(T,SUM, label='SUM')

# # 坐标轴设置
# plt.xlabel("t(s)") 
# plt.ylabel("acceleration(m/s^2)")
# plt.legend()

# # 显示
# plt.show()